Device for the detection of the attitude of auto-vehicles and corresponding method

ABSTRACT

Described herein is a device for detecting the attitude of motor vehicles, which comprises using at least one filter of a complementary type for computing an estimate ({circumflex over (x)} i ) of angles of attitude (θ, φ, Ψ) of the motor vehicle as a function of input signals comprising an acceleration signal (A) and an angular-velocity signal (ω). According to the invention, the device ( 10 ) comprises a plurality of complementary filters ( 12   1   , . . . , 12   n ), each tuned for operating in a specific dynamic range, and a supervisor unit ( 11 ), configured for recognising the dynamic range of the input signals (A, ω) and selecting a corresponding filter ( 12   i ) from said plurality of complementary filters ( 12   1   , . . . , 12   n ).

The present invention relates to a device for detecting the attitude of motor vehicles, which comprises using at least one filter of a complementary type to estimate angles of attitude of the motor vehicle.

There are known in the sector of vehicles, including motor vehicles, inertial navigation systems (INSs) that enable, with respect to known navigation systems, based, for example, on GNSS signals, alternative or complementary navigation means. The INS records relative movements of the vehicle on which it is mounted and, on the basis of these, makes evaluations of the speed of the vehicle and the path followed.

The above INSs comprise inertial measurement units (IMUS) essentially based upon the measurements made by acceleration sensors and gyroscopes. In particular, these INSs use the measurements of accelerometers and gyroscopes to estimate angles of roll, pitch, and yaw of the motor vehicle, which enable evaluation of the attitude thereof.

INSs are based for the most part on extended Kalman filters (EKFs) in order to estimate the above angles of attitude of the vehicle using sensor-fusion techniques. This type of technique involves a high degree of computational complexity, especially if there is available a large number of sensor channels, as well as difficulty of calibration as regards the observability matrix Q and the covariance matrix R of the Kalman filter, and hence a high cost in terms of man hours for its development.

An alternative solution envisages use of complementary filters, which are instead easy to tune.

Various embodiments of complementary filters that can be used for providing an estimator of attitude are illustrated, for example, in the paper by Walter Higgins, “A comparison of complementary and Kalman filtering”, Aerospace and Electronic Systems, IEEE Transactions on (Volume: AES-11, Issue: 3), May 1975. However, complementary filters are valid only for a specific dynamic range, in which the model of the vehicle can be linearly approximated.

The object of the present invention is to provide an improved device that will enable use of filters with a lower degree of complexity, maintaining the performance of extended Kalman filters.

According to the present invention, the above object is achieved thanks to a device for detecting the attitude, as well as to a corresponding method having the characteristics recalled specifically in the ensuing claims.

The invention will now be described with reference to the annexed drawings, which are provided purely by way of non-limiting example and in which:

FIG. 1 is a schematic perspective view of a device described herein;

FIG. 2 is a detailed view of a filter of the device of FIG. 1; and

FIG. 3 is a schematic view of an embodiment of a selector of the device of FIG. 1.

In brief, the solution according to the invention regards a device for detecting the attitude of motor vehicles that comprises using at least one filter of a complementary type to estimate angles of attitude of the motor vehicle, the device including a plurality of complementary filters, each tuned for operating in a specific dynamic range, and a supervisor unit, configured for recognising the dynamic range of the input signals and selecting a corresponding filter in said plurality of complementary filters.

Illustrated in FIG. 1 is a block diagram of an attitude-detection device according to the invention designated as a whole by the reference number 10.

Shown at input to the above attitude-detection device 10 is a signal representing an acceleration A of the motor vehicle, measured by an accelerometer, not illustrated in FIG. 1. The aforesaid acceleration A of the vehicle comprises components of acceleration with amplitudes of acceleration A_(x), A_(y), A_(z) along three mutually perpendicular axes x, y, z, measured, for example, by a triaxial accelerometer device.

Moreover shown at input to said attitude-detection device 10 is a signal representing an angular velocity ω of the vehicle measured by a gyroscope, which is not illustrated in FIG. 1 either. This angular velocity ω also comprises three components, p, q, r, expressed in the co-ordinates of the body, i.e., of the sensor.

The attitude-detection device 10 comprises a bank 12 of complementary filters. This bank 12 of filters comprises a plurality of complementary filters 12 ₁, . . . 12 _(n), each of which receives in parallel the acceleration signal A and the angular-velocity signal ω. Each i-th complementary filter 12 _(i) supplies at output a respective i-th estimate {circumflex over (x)}_(i) of the angles of attitude of the vehicle, i.e., angles of roll θ, pitch φ, and yaw Ψ, as represented in greater detail with reference to FIG. 2.

The attitude-detection device 10 further comprises a supervisor module 11, which also receives at input the acceleration signal A and the angular-velocity signal ω, the velocities of the four wheels w_(s), and the steering angle α and, on the basis of the values of these signals, controls, via a selection signal S, a selector 13 so that it connects one of the outputs {circumflex over (x)}_(i) of the filters 12 _(i) to an output U of the attitude-detection device 10.

The above operation of tuning of each filter 12 _(i) for a different dynamic range is obtained in particular by setting a value of a respective time constant τ of the filter 12 _(i), as described more clearly in what follows with reference to FIG. 2. The filters 12 _(i) are configured with different time constants τ, these different time constants τ rendering each filter 12 _(i) more suited to operating in a different dynamic range, i.e., to supplying an estimate {circumflex over (x)}_(i) of the angles of attitude of the vehicle that is more accurate in that particular dynamic range of the input signals.

The supervisor module 11 is configured for detecting the dynamic range associated to the acceleration signal A and to the angular-velocity signal ω at input generated by the vehicle while it is travelling and for selecting the complementary filter 12 _(i) tuned for the corresponding dynamic range.

In various embodiments, the supervisor module 11 is configured for detecting the level, i.e., the amplitudes A_(x), A_(y), A_(z) of the acceleration signal A and the level, i.e., the amplitudes p, q, r of the angular velocity that can be derived from the angular-velocity signal ω supplied by the gyroscope.

In a preferred embodiment, the supervisor module 11 may comprise, for example stored in a memory of the microcontroller that implements it, a plurality of different models of the vehicle that estimate one or more of the acceleration values A_(x), A_(y), A_(z) and/or of the signals of the gyroscope p, q, r, on the basis of other measured values of dynamic quantities of the vehicle that affect the value of the attitude in order to identify the appropriate complementary filter to be used in the estimate of attitude.

The models stored are different in so far as they take into account different operating conditions of the vehicle, both in terms of dynamic range (for example, range of speeds, acceleration, values of friction) and in terms of type of manoeuvre.

In this connection, FIG. 3 illustrates an example of supervisor 11 comprising a plurality of different vehicle models 11 ₁, . . . , 11 _(n).

The supervisor block 11 further comprises an evaluator block 111, configured for evaluating which of the aforesaid vehicle models 11 ₁, . . . , 11 _(n) makes the best estimate of a current dynamic state of the vehicle. On the basis of the evaluation of the evaluator block 111, i.e., on the basis of the j-th model 11 _(j) that best estimates the dynamic state of the vehicle, the model of the complementary filter 12 ₁, . . . , 12 _(n) to be used is chosen, by issuing a corresponding selection signal S.

In particular, in the example of FIG. 3, each model 11 _(j) supplies its own estimates of the amplitudes of the accelerations Â_(yj), Â_(xj) on the basis of the input data of the velocities of the four wheels w_(s) and on the basis of the steering angle α. Each vehicle model 11 _(j) is configured to estimate in an optimal way the accelerations Â_(yj), Â_(xj) in a given dynamic range of input quantities, i.e., values of the velocities of the four wheels w_(s) and steering angle α, corresponding to given ranges of the dynamic quantities to be estimated, in the specific case the accelerations Â_(yj), Â_(xj), which in turn correspond to a given value of the time constant τ of the filter 12.

Evaluation of the vehicle model 11 _(j) in the evaluation block 111 may, for example, be carried out by measuring an error e_(j) as distance of estimates Â_(x), Â_(y), Â_(z) of the accelerations from the acceleration values Ax, Ay, Az, effectively measured by the inertial platform. In general, this distance may be obtained via calculation of a norm applied to these estimates Â_(x), Â_(y), Â_(z) and to the corresponding measurements A_(x), A_(y), A_(z).

The above norm may be of different types; for example, it may be a norm that calculates a Euclidean distance between quantities.

In a preferred embodiment, the above distance, or, error e_(i) may be obtained as

∫f(Â _(z) −A _(z))² +f(Â _(y) −A _(y))² +f(Â _(x) −A _(x))² dt

i.e., a norm where to the quadratic distances between the components a cost functional f is moreover applied to take into account specific aspects of the vehicle condition, understood as dynamic range, but also as type of manoeuvre and possibly other parameters such as conditions of friction or wind.

The above cost functional f may be implemented, for example, as a weighted measurement, i.e., as a weight applied to the norm or to the measurement of distance, the weights being chosen on the basis of observations on standard manoeuvres carried out by the motor vehicle.

It should be pointed out that the evaluator 111 operates in the first place to calculate the closeness between the estimate and the measurement supplied by each model. In this perspective, the cost functional f represents a possible further degree of freedom that can be applied to the evaluator 111 to render it more flexible in regard to particular vehicle conditions. In theory, the model could be complex to the point of taking into account every parameter of interest for the estimate and not require the above functional, or else a less flexible estimate without the functional may be accepted. Hence, in various embodiments, the aforesaid cost functional f is not present, or may be equal to 1 in the equation appearing above.

In various embodiments, the above cost functional f may be the same for all the models 11 ₁, . . . , 11 _(n).

In various embodiments, the aforesaid cost functional f is different for different models associated to different dynamic ranges, to take into account, for example, the specificity of certain manoeuvres associated to those dynamic ranges and models. There may be applied to the blocks 11 ₁, . . . , 11 _(n) functionals f_(j) that represent, for example, a vehicle condition with low coefficient of friction, a condition of highspeed ring, or a condition of twist of the steering wheel.

In the example specifically described in FIG. 3, as has been said the vehicle model 11 ₁ may use a cost functional f that calculates an error e_(i) corresponding to

∫f(Â _(yj) −A _(y))² +f(Â _(xj) −A _(x))² dt

where Â_(yj,xj) is an estimate of the acceleration along the axes x and y for each model 11 _(j), A_(y,x) is the corresponding measurement made by the inertial platform, and f is the cost functional.

The minimum min(e₁, e₂, . . . , e_(n)) of the error e_(j) evaluated in block 111, determines the value assumed by the selection signal S, which corresponds to the choice of the model 11 _(i) associated to a respective dynamic range DR_(i) that best estimates the quantity at its input, and hence to the choice of the complementary filter 12 ₁, . . . , 12 _(n) to be used dynamically that best operates in the corresponding dynamic range DR_(i).

Each vehicle model 11 _(j) with j=1, . . . , n defines a particular vehicle condition whereby a given error e_(j) is the minimum for all the models considered if the accelerations and angular velocities measured derive from the vehicle condition associated to the corresponding model 11 _(j). The vehicle model 11 _(j) identifies a particular dynamic range of the vehicle quantities A_(x), A_(y), A_(z), of the acceleration signal A and of the angular velocities p, q, r measured. Furthermore, for each vehicle model 11 _(j) there exists just one filter 12 _(i) with a time constant τ defined for that particular vehicle condition.

In particular, for example, stored in the supervisor module 11 is a look-up table or other data structure that sets each of the vehicle models 11 ₁, . . . , 11 _(n) in relation with one of the filters 12 ₁, . . . , 12 _(n). It is envisaged to construct empirically the above look-up table by identifying, on the basis of the correctness of the estimator {circumflex over (x)}_(i) supplied at output, the value of time constant τ that corresponds to a given dynamic range, i.e., that enables a correct estimate of the angle of attitude, for example within a pre-defined error value. Hence, in the look-up table the vehicle model 11 _(j) associated to that given dynamic range is set in relation with the filter with time constant that enables correct estimation of the attitude in the context of the system 10. When the minimum min(e₁, e₂, . . . , e_(n)) of the error e_(j) indicates that a certain model 11 _(j) is the most suited to the dynamic range of the acceleration and/or gyroscopic signals at input, by accessing the look-up table with the index of that model 11 _(j) at output there is obtained, as selection signal S, the index of the filter 12 _(i) with a time constant τ suited to its dynamic range and hence to be selected to obtain the best estimate of attitude.

The supervisor module 11 is obtained via a microprocessor module, for example the ECU of the motor vehicle. Also the selector 14 may be integrated within this microprocessor.

Illustrated in FIG. 2 is a detailed diagram of a complementary filter 12 _(i). This complementary filter 12 _(i) comprises a module 121 for computing the angle of inclination, or tilt, which receives at input the acceleration signal A and supplies calculated values of the angles of roll θ, pitch φ, and yaw Ψ. This is achieved according to the following relations, in themselves known:

$\begin{matrix} {{\theta = {\arctan \left( \frac{A_{x}}{\sqrt{A_{y}^{2} + A_{x}^{2}}} \right)}}{\phi = {\arctan \left( \frac{A_{y}}{\sqrt{A_{x}^{2} + A_{z}^{2}}} \right)}}{\psi = {\arctan \left( \frac{\sqrt{A_{x}^{2} + A_{y}^{2}}}{A_{z}} \right)}}} & (1) \end{matrix}$

The complementary filter 12 _(i) further comprises a module for computing attitude rates 126, which receives at input the angular-velocity signal ω and performs the calculation of the values of the first derivatives of the angles of roll θ, pitch φ, and yaw Ψ, i.e., {dot over (θ)}, {dot over (φ)}, {dot over (Ψ)}. The attitude-rate computing module 126 makes, in particular, the following calculation:

$\begin{matrix} \overset{.}{\varphi = {p + {\tan \; {\theta \left( {{q\mspace{11mu} \sin \; \psi} + {r\; \cos \; \psi}} \right)}}}} & (2) \\ {\overset{.}{\theta} = {{q\; \cos \; \phi} - {r\; \sin \; \phi}}} & \; \\ {\overset{.}{\psi} = \frac{{q\; \sin \; \phi} + {r\; \cos \; \phi}}{\cos \; \theta}} & \; \end{matrix}$

From the angular velocities p, q, r measured by the gyroscope and from the values of the angles of attitude, or Euler angles, it is possible to measure the attitude rates, i.e., the first derivatives {dot over (θ)}, {dot over (φ)}, {dot over (Ψ)} of the Euler angles using Eqs. (2), which are in themselves known.

In particular, the above Eqs. (2) may be written in matrix form as:

$\begin{matrix} {{\begin{matrix} \overset{.}{\phi} \\ \overset{.}{\theta} \\ \overset{.}{\psi} \end{matrix}} = {{A^{- 1}\left( {\phi,\theta,\psi} \right)}{\begin{matrix} p \\ q \\ r \end{matrix}}}} & (3) \end{matrix}$

taking into account that

$\begin{matrix} {{A^{- 1}\left( {\phi,\theta,\psi} \right)} = {\begin{matrix} {\cos \; (\theta)} & 0 & {\sin \; (\theta)} \\ {\sin \; (\theta)\tan \; (\phi)} & 1 & {{- \cos}\; (\theta){\tan (\phi)}} \\ {- \frac{\sin (\theta)}{\cos \; (\phi)}} & 0 & \frac{\cos \; (\theta)}{\cos \; (\phi)} \end{matrix}}} & (4) \end{matrix}$

The relations (3) and (4) represent the construction of block 126, the matrix A⁻¹ being the matrix of rotation that enables calculation of the angular velocities {dot over (θ)}, {dot over (φ)}, {dot over (Ψ)} in the vehicle reference system starting from the angular velocities p, q, r measured by the gyroscope (sensor reference system) and from the estimate of the angles of roll θ and pitch φ.

The calculated values of the angles of roll θ, pitch φ, and yaw Ψ are supplied by the tilt-angle computing module 121 at input, as set-point, to a control loop that as a whole constitutes a complementary lowpass filter and a complementary highpass filter, through a derivator block 123, with time constant τ, and an integrator block 125. Specifically, the aforesaid calculated values of the angles of roll θ, pitch φ, and yaw Ψ are supplied at input, as set-point, to an adder node 122, which computes the difference with respect to the estimate {circumflex over (x)}_(i) at output from the complementary filter 12 _(i). The corresponding difference signal D is supplied at input to the derivator 123, which has a time constant τ. The output of the derivator 123 is supplied to a second adder node 122, which adds it to the attitude rates {dot over (θ)}, {dot over (φ)}, {dot over (Ψ)} supplied at output from the attitude-rate computing module 126. The corresponding addition signal M is supplied to the integrator 125, which supplies at output the estimate of the angle of attitude of the vehicle {circumflex over (x)}_(i).

Hence, the complementary filter 12 _(i) is in this way used for combining two independent measurements that are in themselves noisy, i.e., the acceleration signal A supplied by the accelerometer and the angular-velocity signal ω supplied by the gyroscope, where each measurement is corrupted by different types of spectral noise. The filter 12 _(i) provides an estimate of the real angle of attitude of the vehicle {circumflex over (x)}_(i) via the two complementary highpass and lowpass filters with time constant τ.

The values of the angles of roll θ, pitch φ, and yaw Ψ are supplied already directly by the tilt-angle computing module 121 on the basis of the acceleration signal A. This measurement, however, is accurate only at low dynamics.

The time constant τ according to a main aspect of the solution described herein is set different for each filter 12 _(i) so as to tune the respective filter to a different dynamic range of the input signals A and ω.

Hence, from what has been described above, the advantages of the solution proposed emerge clearly.

The device for detecting the attitude of motor vehicles via the use of a bank of complementary filters enables use of filters with a lower degree of complexity, maintaining the performance of extended Kalman filters.

The device for detecting the attitude of motor vehicles, as compared to the extended Kalman filter used in applications linked to the dynamics of the motor vehicle, guarantees levels of performance that fully meet the specifications, but with a reduction of the complexity of tuning of the filters and of the use of computational resources.

The device for detecting the attitude of motor vehicles based upon an approach of the ‘Data Fusion’ type combines the measurements of gyroscopic and acceleration sensors present on board the vehicle in the framework of an inertial navigation system (INS), increasing and advantageously enriching each of the above measurements, so that there is obtained, for the vehicle, a powerful system of positioning of the attitude on three axes.

The estimate of attitude can be used for continuous mobile positioning obtained with the IMU.

The stable relative position supplied by the INS can then be used as bridge information for covering periods of time during which a navigator based upon GNSS signals, in particular GPS signals, receives degraded signals or these signals are not available. 

1. A device for detecting the attitude of motor vehicles that comprises using at least one filter of a complementary type for computing an estimate ({circumflex over (x)}_(i)) of angles of attitude (θ, φ, Ψ) of the motor vehicle as a function of input signals comprising an acceleration signal (A) and an angular-velocity signal (ω), said device being characterized in that it comprises a plurality of complementary filters (12 ₁, . . . , 12 _(n)), each tuned for operating in a specific dynamic range, and a supervisor module (11), configured for recognising the dynamic range of the input signals (A, ω) and selecting (S) a corresponding filter (12 i) in said plurality of complementary filters (12 ₁, . . . , 12 _(n)).
 2. The device according to claim 1, characterized in that said supervisor module (11) receives at input the acceleration signal (A) and the angular-velocity signal (ω) measured by respective sensors and, on the basis of the values of said input signals (A, ω), controls a selector (13) so that it selects one of the estimates ({circumflex over (x)}_(i)) available at the outputs of said plurality of complementary filters (12 ₁, . . . , 12 _(n)) as output (U) of the attitude-detection device (10).
 3. The device according to claim 2, characterized in that said supervisor module (11) is configured for detecting the level (Ax, Ay, Az) of the acceleration signal (A) and the level (p, q, r) of the angular velocity (ω) and selecting, on the basis of said levels, the complementary filter (12 _(i)) to be used in the estimate of attitude from said plurality of complementary filters (12 ₁, . . . , 12 _(n)).
 4. The device according to claim 2 or claim 3, characterized in that said supervisor module (11) comprises a plurality of models (11 ₁, . . . , 11 _(n)) of the vehicle defined as a function of dynamic quantities of the vehicle (A, ω, w_(s), α), each of said models generating an estimate of the dynamic quantities of the vehicle (Â_(x), Â_(y), Â_(z)), in particular a plurality of different models (11 ₁, . . . , 11 _(n)) of the vehicle that estimate one or more of the acceleration values (Ax, Ay, Az) and/or of the gyroscope signals (p, q, r) on the basis of other measured values of dynamic quantities (A, ω, w_(s), α) of the vehicle that affect the value of the attitude, said supervisor module (11) comprising a module (111) for evaluating the model in said plurality of models (11 ₁, . . . , 11 _(n)) that best estimates the current condition (DR_(i)) of the vehicle, on the basis of said evaluation there being carried out said operation of selecting (S) a corresponding filter (12 _(i)) from said plurality of complementary filters (12 ₁, . . . , 12 _(n)).
 5. The device according to claim 4, characterized in that said evaluation module (111) is configured for measuring an error (e₁, . . . , e_(n)) of each model (11 ₁, . . . , 11 _(n)) calculated as distance of estimates Â_(x), Â_(y), Â_(z)) of each model (11 ₁, . . . , 11 _(n)) from the values (Ax, Ay, Az) effectively measured and for identifying the minimum value (min(e₁, e₂, . . . e_(n))) of said error (e₁, . . . , e_(n)) and the corresponding model (11 ₁, . . . , 11 _(n)).
 6. The device according to claim 3 or claim 4, characterized in that said evaluation module (111) is configured for applying in the calculation of said distances (e₁, . . . , e_(n)) a cost functional (f) associated to a given condition of the vehicle, in particular a weighted measurement, the weights of which are chosen on the basis of observations on manoeuvres made by the motor vehicle.
 7. The device according to any one of claims 3 to 6, characterized in that said complementary filter (12 _(i)) comprises a module (121) for computing the angle of inclination, or tilt, which receives at input the acceleration signal (A) and is configured for supplying calculated values of the angles of roll (θ), pitch (φ), and yaw (Ψ), and a module for computing attitude rates (126), which receives at input the angular-velocity signal (ω) and is configured for computing the values of the first derivatives ({dot over (θ)}, {dot over (φ)}, {dot over (Ψ)}) of said angles of roll (θ), pitch (φ), and yaw (Ψ), said tilt-angle computing module (121) and said attitude-rate computing module (126) supplying said outputs to a control loop (122, 123, 124, 125) comprising complementary filters (123, 125).
 8. The device according to claim 7, characterized in that the output of said tilt-angle computing module (121) is supplied as set-point to the control loop (122, 123, 124, 125) for generating a difference signal (D) with respect to the estimate of attitude at output from the control loop, while the signal at output from the attitude-rate computing module (126) is added to the output of a derivator block (123), which receives at input said difference signal (D), a resulting addition signal being supplied as input to an integrator (125), which generates said estimate of the angle of attitude of the vehicle ({circumflex over (x)}_(i)).
 9. The device according to claim 7 or claim 8, characterized in that said control loop is configured for constituting a complementary lowpass filter and a complementary highpass filter, the time constant (τ) of which is set for tuning the filter with respect to a given dynamic range.
 10. The device according to at least one of the preceding claims, characterized in that it is comprised in an inertial navigation system of a motor vehicle.
 11. A method for estimating the attitude of a motor vehicle that implements the operations performed by the device according to any one of claims 1 to
 10. 